/*
 * tarm_runPV.c
 * function: pv axis linkage function + accelate function
 *  s作用：PV轴联动函数+液体搬运优化函数
 */

#include <stdlib.h>

// #include "tarm_timer.h"
#include "TARM_Middleware/tarm_math.h"

#include "TARM_Hardware/tarm_io.h"
#include "TARM_Hardware/tarm_limit.h"
#include "TARM_Hardware/tarm_pwm.h"
#include "TARM_Hardware/tarm_baseTimer.h"

#include "TARM_Application/tarm_init.h"
#include "TARM_Application/tarm_run.h"

#include <math.h>
#include <stdlib.h>
#include <stdio.h>

// #include "hardware/closeloop/CloseLoop.h"




/************************************************************
*	配合point_to_point_xy_pvthr使用
************************************************************/
//******************************
//@brief 		cal square 计算平方
//@param[in]
//@return
//******************************
static inline double hf_square(double x)
{
    return x*x;
}

//******************************
//@brief 		判断下一步哪个轴该走，是否到达
//				1-handle pv timer interrupt;
//				2-first start;
//@param[in]
//@return
//******************************
int handle_pvaxis(double *retu_lengthPerAngle, int *retu_whichOne)
{
	//判断哪个轴动比较合适
	int h_whichOne=0;
	//走过的路程
	double h_s =0;
	double h_lengthPerAngle = 0;
	//判断步进加减
	double t_calPrimeStep=0,t_calViceStep=0;
	//1.1.2-构造结构体
	struct _TARM_PVValStruct h_armStruct1={0,0,0,0},h_armStruct2={0,0,0,0};
	//
	double h_judgeAngleDis1,h_judgeAngleCur1,h_judgeAngleMinus1,
		h_judgeAngleDis2,h_judgeAngleCur2,h_judgeAngleMinus2;
	//
	double h_allDisSqu,h_curDisSqu;

	//1.1-
	//1.1.1-判断步进加减
//	double t_calPrimeStep=0,t_calViceStep=0;
	if(g_TARM_RunRecord.pv_distance.p>g_TARM_RunRecord.pv_current.p){
		t_calPrimeStep += g_TARM_Factor.step_p;
		X_DIR_SET;
	}
	else{
		t_calPrimeStep -= g_TARM_Factor.step_p;
		X_DIR_RESET;
	}
	if(g_TARM_RunRecord.pv_distance.v>g_TARM_RunRecord.pv_current.v){
		t_calViceStep += g_TARM_Factor.step_v;
		Y_DIR_SET;
	}
	else{
		t_calViceStep -= g_TARM_Factor.step_v;
		Y_DIR_RESET;
	}

	//1.1.2-构造结构体
	h_armStruct1.p=g_TARM_RunRecord.pv_current.p+t_calPrimeStep;
	h_armStruct1.v=g_TARM_RunRecord.pv_current.v;
	h_armStruct2.p=g_TARM_RunRecord.pv_current.p;
	h_armStruct2.v=g_TARM_RunRecord.pv_current.v+t_calViceStep;

	//1.2.1-走p轴
	//转化出笛卡尔坐标系
	angle_to_xy(h_armStruct1.p, h_armStruct1.v, &h_armStruct1.x, &h_armStruct1.y);
	h_judgeAngleDis1 = atan2(g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_start.y,
			g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_start.x);  //需要角度
	h_judgeAngleCur1 = atan2(h_armStruct1.y-g_TARM_RunRecord.pv_start.y,
			h_armStruct1.x-g_TARM_RunRecord.pv_start.x);        //步进角度
	h_judgeAngleMinus1 = fabs(h_judgeAngleDis1-h_judgeAngleCur1);    //角度差
	//当跨跃了180度时会发生突变，产生超过180度的值，需要换成对角
	if(h_judgeAngleMinus1>3.1415)
		h_judgeAngleMinus1 = 2*3.1415-h_judgeAngleMinus1;

	//1.2.2-走v轴
	angle_to_xy(h_armStruct2.p, h_armStruct2.v, &h_armStruct2.x, &h_armStruct2.y);
	h_judgeAngleDis2 = atan2(g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_start.y,
			g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_start.x);
	h_judgeAngleCur2 = atan2(h_armStruct2.y-g_TARM_RunRecord.pv_start.y,
			h_armStruct2.x-g_TARM_RunRecord.pv_start.x);
	h_judgeAngleMinus2 = fabs(h_judgeAngleDis2-h_judgeAngleCur2);
	//当跨跃了180度时会发生突变，产生超过180度的值，需要换成对角
	if(h_judgeAngleMinus2>3.1415)
		h_judgeAngleMinus2 = 2*3.1415-h_judgeAngleMinus2;

	//1.3-判断是哪个轴运动
	h_whichOne = h_judgeAngleMinus1<h_judgeAngleMinus2?1:2;
	//如果某个轴已经到位了，就不要再动了
	if(fabs(g_TARM_RunRecord.pv_current.v - g_TARM_RunRecord.pv_distance.v) <= g_TARM_Factor.step_v &&
			fabs(g_TARM_RunRecord.pv_current.p - g_TARM_RunRecord.pv_distance.p) > g_TARM_Factor.step_p)
		h_whichOne=1;
	else if(fabs(g_TARM_RunRecord.pv_current.v - g_TARM_RunRecord.pv_distance.v) > g_TARM_Factor.step_v &&
			fabs(g_TARM_RunRecord.pv_current.p - g_TARM_RunRecord.pv_distance.p) <= g_TARM_Factor.step_p)
		h_whichOne=2;

	*retu_whichOne = h_whichOne;

	//2.1-算投影
	//走过的路程
	//		double h_s =0;
	//		double h_lengthPerAngle = 0;
	//投影
	if(h_whichOne == 1){
		h_s = hf_square(h_armStruct1.x-g_TARM_RunRecord.pv_current.x)+hf_square(h_armStruct1.y-g_TARM_RunRecord.pv_current.y);
//		h_s = sqrt(h_s);
//		h_lengthPerAngle = cos(h_judgeAngleMinus1)*h_s;
		h_s = tarm_sqrt(h_s);
		h_lengthPerAngle = tarm_cos(h_judgeAngleMinus1)*h_s;
	}
	else if(h_whichOne == 2){
		h_s = hf_square(h_armStruct2.x-g_TARM_RunRecord.pv_current.x)+hf_square(h_armStruct2.y-g_TARM_RunRecord.pv_current.y);
//		h_s = sqrt(h_s);
//		h_lengthPerAngle = cos(h_judgeAngleMinus2)*h_s;
		h_s = tarm_sqrt(h_s);
		h_lengthPerAngle = tarm_cos(h_judgeAngleMinus2)*h_s;
	}

	//2.2-更新角度数值
	if(h_whichOne == 1){
		g_TARM_RunRecord.pv_current.x = h_armStruct1.x;
		g_TARM_RunRecord.pv_current.y = h_armStruct1.y;
		g_TARM_RunRecord.pv_current.p = h_armStruct1.p;
		g_TARM_RunRecord.pv_current.v = h_armStruct1.v;
	}
	else if(h_whichOne == 2){
		g_TARM_RunRecord.pv_current.x = h_armStruct2.x;
		g_TARM_RunRecord.pv_current.y = h_armStruct2.y;
		g_TARM_RunRecord.pv_current.p = h_armStruct2.p;
		g_TARM_RunRecord.pv_current.v = h_armStruct2.v;
	}

	//传进来的是ms/cm,h_lengthPerAngle单位也是cm
	//*inpa_delayTimeMs = (*inpa_delayTimeMs)*h_lengthPerAngle;
	*retu_lengthPerAngle = h_lengthPerAngle;

	//3-判断是否达到
	if(fabs(g_TARM_RunRecord.pv_current.v - g_TARM_RunRecord.pv_distance.v) <= g_TARM_Factor.step_v &&
			fabs(g_TARM_RunRecord.pv_current.p - g_TARM_RunRecord.pv_distance.p) <= g_TARM_Factor.step_p)
		return -1;
	else {
		h_allDisSqu = hf_square(g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_start.x)
									+hf_square(g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_start.y);
		h_curDisSqu = hf_square(g_TARM_RunRecord.pv_distance.x-g_TARM_RunRecord.pv_current.x)
									+hf_square(g_TARM_RunRecord.pv_distance.y-g_TARM_RunRecord.pv_current.y);
//		return (int)(sqrt(h_curDisSqu)/sqrt(h_allDisSqu)*100);
		return (int)(tarm_sqrt(h_curDisSqu)/tarm_sqrt(h_allDisSqu)*100);
	}
}

//******************************
//@brief 		calculate pass parameter 计算传递参数
//@param[in]
//@return		the percentage from 0 to 1 ,double
//******************************
double SpeedFactorTransferFunc(const double inpu_percentage,
		const double inpu_commandSpeedFactor,
		const double inpu_sysPreSetSpeedFactor)
{
	const double h_parA=(1/inpu_commandSpeedFactor)-(1/inpu_sysPreSetSpeedFactor);
	const double h_parB=(1/inpu_commandSpeedFactor);
	return 1/(h_parB-h_parA*cos(3.1415/50*inpu_percentage));
//	const double h_parA=tarm_inv(inpu_commandSpeedFactor)-tarm_inv(inpu_sysPreSetSpeedFactor);
//	const double h_parB=tarm_inv(inpu_commandSpeedFactor);
//	return tarm_inv(h_parB-h_parA*tarm_cos(3.1415/50*inpu_percentage));
}

/************************************************************
*	已知xy，调用函数即可驱动机械臂运动至目标位置，带变速，传递函数变速
*	传入参数：	x（x的值）；y（y的值）；speed_ms（速度的倒数，ms/cm）
*	返回值：	-1（出错）；0（成功）
*	运动范围：以(20,0)为圆心，15cm为半径 + >=-10cm
*	注释：	如果传入的xy超过范围，将返回-1，g_TARM_RunRecord.pv_distance.x不变
			speed方面，speed_ms是变速的平均速度参数
************************************************************/

//******************************
//@brief 		有传递参数(SpeedFactorTransferFunc)的PV轴轮询服务函数
//				0-handle pv timer interrupt; (h_pullLowFlag=0)
//				1-first start;(h_pullLowFlag=1)
//@param[in]
//@return		no use
//******************************
int handle_pvMainLoop(double x,double y,uint16_t speed_ms)
{
	static int h_pullLowFlag=0;
	int h_whichOne=0;
//	double h_g_TARM_RunRecord.SpeedReci_MsPerCm=g_TARM_RunRecord.SpeedReci_MsPerCm;
	double h_length=0,h_delayTime=0;
	int percentage_recorder=0;
	double t_lastDelayTime=0;

	//第一次启动，参数，下拉标志清零
	// h_pullLowFlag = 0;

	//1、判断x、y是否超过界限
	if(y >= -10){
		double counter = (x-20)*(x-20) + y*y;
		if(counter >= 992){
			return -1;
		}
	}
	else
		return -1;

	//2、如果正确，修改目标值
	g_TARM_RunRecord.pv_distance.x = x;
	g_TARM_RunRecord.pv_distance.y = y;
	//调用xy_des_to_angle自动同步目标【xy-角度关系】
	xy_des_to_angle();

//	if(InitFlag==1 && CloseLoopSingle.CloseLoopFlag!=Open_CloseLoop)	//初始化完成后才闭环，非必须是CloseLoopFlag才闭环
//			CloseLoopUpdate();

	//3、修改起始值
	g_TARM_RunRecord.pv_start.p = g_TARM_RunRecord.pv_current.p;
	g_TARM_RunRecord.pv_start.v = g_TARM_RunRecord.pv_current.v;
	g_TARM_RunRecord.pv_start.x = g_TARM_RunRecord.pv_current.x;
	g_TARM_RunRecord.pv_start.y = g_TARM_RunRecord.pv_current.y;

	g_TARM_RunRecord.SpeedReci_MsPerCm = speed_ms;

	//4、调用handle
	while (1)
	{
		//1、调用函数，判断是否到达
		percentage_recorder = handle_pvaxis(&h_length, &h_whichOne);

		//保证到这里最少经过>1/4+>100ms的延时

		//3、把所有STP IO拉低
		X_STP_RESET;
		Y_STP_RESET;

		//保证到这里最少还>1/4+>100ms的延时

		//到达
		if (percentage_recorder == -1)
		{
//			if (InitFlag == 1 && CloseLoopSingle.CloseLoopFlag != Open_CloseLoop) //初始化完成后才闭环，非必须是CloseLoopFlag才闭环
//			{
//				delay_ms(_CloseloopDelayMsValue_);
//				CloseLoopUpdate();
//			}

			ReportStruct.Uart1ReportFlag.ExecuteFinish = 1;
			ReportStruct.Uart2ReportFlag.ExecuteFinish = 1; //上报
			break;
		}
		//2、没有到达则 修改定时器值，开启定时器 其中值为延时时间的一半，延时两次
		else
		{
			TimePV_WaitForFinishAtLeast(100);
			// h_pullLowFlag = 1;
			if (h_whichOne == 1)
			{
				X_STP_SET;
			}
			else if (h_whichOne == 2)
				Y_STP_SET;

			//判断运行速度因素
			h_delayTime = SpeedFactorTransferFunc(percentage_recorder, g_TARM_RunRecord.SpeedReci_MsPerCm, 1000);
			t_lastDelayTime = h_delayTime * h_length * (1000);
			TimePV_StartDelayCounter(t_lastDelayTime); //ms/2->us
		}

	}

	return 0;
}

